Project Summary

In this section we are going to summarise what we have done for our project and provide all the code, images and videos that we have made. As we have explained in the previous pages (week 1-4), we have designed a robotic arm that consists of four servo motors connected together with 3 pin cables and different shaped frames. Those servos are internally built with pressure and heat sensors to get feedbacks on their state, a potentiometer to know their position, and a little Arduino which can receive commands and execute them.
The main purpose of the robotic arm is to show that it can be manipulated as if it was a real human arm. It mimic human constraints as much as possible.

The robotic arm has been set to have constraints so that it does not execute invalid moves which a human can not do. For the project we have designed a simple housing that allows the arm to hang just like a relaxed human arm and we have dressed it up, so that it looks more fun and realistic. We use a C++ graphical interface using the library Qt, and URBIscript which uses a specific Urbi language to interact with the servo motors. We use the library bioloid with Urbi to make the interaction easier.

During the coding, we have experienced quite a few problems, but all were solved on time. The first problem was to create an interface with Qt which can interact with the Urbi server. However with a bit of research, we have managed to find a simple solution. As Urbi is a server, we have set it up to listen on a certain port. Then with the C++ program, we created a function to send it messages using a socket connection. The initial idea was to use the serial port but it appeared not to be a good idea because Urbi was already using it.

Another problem that we encountered was to stop the record script. The script being executed in the foreground and in an infinite loop, we were not able to stop it. However again with a bit of research we found some functions which allow us to execute it in background and so to execute commands to stop it.

The last problem we had was to take in account the constraints we fixed to respect a human arm. In fact, the recorder did not care about them and recorded any given movement. We finally found the solution by controlling whether a movement should be played while reading the recorded positions. Only the position inside the boundaries are sent to the servo motors. The others are ignored.

To interact with the robot, we use two analog inputs, namely a mouse and a keyboard. Obviously, we can use any controller connected to the computer, like a joystick or a trackball.
The mouse allows the user to interact with the graphical interface and press buttons that we have designed to send specific messages to Urbi which react to them by either playing already recorded scripts, playing a demo, reset the servos of the robotic arm or record your own script and then play it straight away. The keyboard was used to manually control the robotic arm. It allows the user to press different keys to trigger each servo individually and try out different kinds of movements without any recorded script.

There are two different outputs in this project: the motors and the screen. The motors act according to an order sent by Urbi. They make the arm move and allow a physical output of the commands executed. On another side, the user might want to control the internal state of the program. That is why we also display some messages on the server terminal so that an output of what is happening is accessible. A simple user is not supposed to interact with the robot outside of the graphical interface. But an experimented user or the owner of the robot might need it.

Below are the pictures that we have taken for the project along with the presentation video and all the fully explained and commented code that we have written.























Following is the code used to create the graphical interface. You can also download it from the links below:
Urbi files
C++ files

main.cpp
#include <QtGui/QApplication>
#include "mainwindow.h"

int main(int argc, char *argv[])
{
QApplication a(argc, argv);
MainWindow w;
w.show();

return a.exec();
}

Here is the header file for the mainWindow.cpp that is used to define the objects and call the libraries and other files needed, such as the socketConnecton.hpp that is required in order to establish a connection.
mainWindow.h
#ifndef MAINWINDOW_H
#define MAINWINDOW_H

#include <QMainWindow>
#include <QKeyEvent>
#include "socketConnection.hpp"

namespace Ui {
class MainWindow;
}

class MainWindow : public QMainWindow
{
Q_OBJECT

public:
explicit MainWindow(QWidget *parent = 0);
virtual void keyPressEvent ( QKeyEvent * event );
~MainWindow();

private slots:
void on_reset_clicked();

void on_demo_clicked();

void on_record_clicked();

void on_play_clicked();

void on_play1_clicked();

void on_play2_clicked();

void on_play3_clicked();

private:
Ui::MainWindow *ui;
bool stopRecording;
};

#endif MAINWINDOW_H

This is where the buttons of the keyboard and the user interfaces buttons are defined and called. When a button is pressed, a connection is established that sends a signal through the port that contains a specific functions that we call inside the urbiscript code.
mainWindow.cpp
#include "mainwindow.h"
#include "ui_mainwindow.h"

MainWindow::MainWindow(QWidget *parent) :
QMainWindow(parent),
ui(new Ui::MainWindow)
{
ui->setupUi(this);
stopRecording = false;
}

MainWindow::~MainWindow()
{
delete ui;
}

void MainWindow::keyPressEvent(QKeyEvent *event)
{
switch(event->key()){
case (Qt::Key_S):
socketConnection("s1_move_down");
break;

case(Qt::Key_Z):
socketConnection("s1_move_up");
break;

case (Qt::Key_Q):
socketConnection("s2_move_down");
break;

case(Qt::Key_D):
socketConnection("s2_move_up");
break;

case (Qt::Key_I):
socketConnection("s3_move_up");
break;

case(Qt::Key_K):
socketConnection("s3_move_down");
break;

case (Qt::Key_J):
socketConnection("e_move_down");
break;

case(Qt::Key_L):
socketConnection("e_move_up");
break;

case(Qt::Key_Escape):
close();
break;

case(Qt::Key_W):
socketConnection("stopRecording");
break;
}
}

void MainWindow::on_reset_clicked()
{
socketConnection("reset");
}

void MainWindow::on_demo_clicked()
{
socketConnection("demo");
}

void MainWindow::on_play_clicked()
{
socketConnection("play");
}

void MainWindow::on_record_clicked()
{
socketConnection("record");
}

void MainWindow::on_play1_clicked()
{
socketConnection("play1");
}

void MainWindow::on_play2_clicked()
{
socketConnection("play2");
}

void MainWindow::on_play3_clicked()
{
socketConnection("play3");
}

This is the header file which uses the libraries required for the socketConnection.cpp file which is used for establishing a connection.
socketConnection.hpp
#ifndef SOCKETCONNECTION_HPP
#define SOCKETCONNECTION_HPP

#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <string>


#define INVALID_SOCKET -1
#define SOCKET_ERROR -1
#define closesocket(s) close(s)
#define PORT 5353

typedef int SOCKET;
typedef struct sockaddr_in SOCKADDR_IN;
typedef struct sockaddr SOCKADDR;


using namespace std;

class socketConnection
{
private:
SOCKET sock;
SOCKADDR_IN sin;
int erreur;
bool connected;
public:
socketConnection(string message);
};

#endif SOCKETCONNECTION_HPP

the socketConnection is used to create a link or a network between urbi script and the serial communication so that commands can be sent through the user interface
socketConnection.cpp
#include "socketConnection.hpp"

socketConnection::socketConnection(string message){
int erreur = 0;

SOCKET sock;
SOCKADDR_IN sin;

if(!erreur)
{
/* socket creation */
sock = socket(AF_INET, SOCK_STREAM, 0);

/* connection config */
sin.sin_addr.s_addr = inet_addr("127.0.0.1");
sin.sin_family = AF_INET;
sin.sin_port = htons(PORT);

/* if the program can connect */
if(connect(sock, (SOCKADDR*)&sin, sizeof(sin)) != SOCKET_ERROR){
printf("Connected on server %s on port %d\n", inet_ntoa(sin.sin_addr), htons(sin.sin_port));
cout << message.length();
send(sock, message.c_str(), message.length(),0);
}
else
printf("Connection refused\n");

/*close the socket*/
closesocket(sock);
}
}

The code below represents the user interface code which defines the names and size of the buttons and their location. Most of the code is automatically generated since it uses drag and drop function, however linking the code does require some coding which is used later
mainWindow.ui
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>MainWindow</class>
<widget class="QMainWindow" name="MainWindow">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>544</width>
<height>418</height>
</rect>
</property>
<property name="windowTitle">
<string>MainWindow</string>
</property>
<widget class="QWidget" name="centralWidget">
<widget class="QWidget" name="gridLayoutWidget">
<property name="geometry">
<rect>
<x>9</x>
<y>9</y>
<width>531</width>
<height>353</height>
</rect>
</property>
<layout class="QGridLayout" name="gridLayout">
<item row="1" column="3" rowspan="13">
<widget class="Line" name="line">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
</widget>
</item>
<item row="8" column="5">
<widget class="QSlider" name="shoulder3">
<property name="enabled">
<bool>false</bool>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item row="13" column="5">
<widget class="QSlider" name="elbow">
<property name="enabled">
<bool>false</bool>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item row="1" column="2">
<spacer name="horizontalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Fixed</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="4" column="4">
<widget class="QLabel" name="label_2">
<property name="text">
<string>Shoulder 2</string>
</property>
</widget>
</item>
<item row="12" column="4">
<widget class="QLabel" name="label_4">
<property name="text">
<string>Elbow</string>
</property>
</widget>
</item>
<item row="5" column="5">
<widget class="QSlider" name="shoulder2">
<property name="enabled">
<bool>false</bool>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item row="7" column="4">
<widget class="QLabel" name="label_3">
<property name="text">
<string>Shoulder 3</string>
</property>
</widget>
</item>
<item row="1" column="4">
<widget class="QLabel" name="label">
<property name="text">
<string>Shoulder 1</string>
</property>
</widget>
</item>
<item row="2" column="6">
<spacer name="horizontalSpacer_3">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Minimum</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="2" column="4">
<spacer name="horizontalSpacer_2">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="1" column="1">
<widget class="QPushButton" name="play1">
<property name="cursor">
<cursorShape>PointingHandCursor</cursorShape>
</property>
<property name="text">
<string>Play script 1</string>
</property>
</widget>
</item>
<item row="11" column="4" colspan="2">
<widget class="Line" name="line_2">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item row="6" column="4" colspan="2">
<widget class="Line" name="line_3">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item row="3" column="4" colspan="2">
<widget class="Line" name="line_4">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item row="2" column="5">
<widget class="QSlider" name="shoulder1">
<property name="enabled">
<bool>false</bool>
</property>
<property name="minimum">
<number>-200</number>
</property>
<property name="maximum">
<number>160</number>
</property>
<property name="singleStep">
<number>10</number>
</property>
<property name="value">
<number>-147</number>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item row="1" column="0">
<spacer name="horizontalSpacer_4">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Fixed</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="2" column="1">
<widget class="QPushButton" name="play2">
<property name="cursor">
<cursorShape>PointingHandCursor</cursorShape>
</property>
<property name="text">
<string>Play script 2</string>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QPushButton" name="play3">
<property name="cursor">
<cursorShape>PointingHandCursor</cursorShape>
</property>
<property name="text">
<string>Play script 3</string>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QPushButton" name="record">
<property name="cursor">
<cursorShape>PointingHandCursor</cursorShape>
</property>
<property name="text">
<string>Record</string>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QPushButton" name="play">
<property name="cursor">
<cursorShape>PointingHandCursor</cursorShape>
</property>
<property name="text">
<string>Play</string>
</property>
</widget>
</item>
<item row="5" column="1">
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
<item row="9" column="1">
<spacer name="verticalSpacer_2">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
<item row="12" column="1">
<widget class="QPushButton" name="demo">
<property name="cursor">
<cursorShape>PointingHandCursor</cursorShape>
</property>
<property name="text">
<string>Demo</string>
</property>
</widget>
</item>
<item row="13" column="1">
<widget class="QPushButton" name="reset">
<property name="cursor">
<cursorShape>PointingHandCursor</cursorShape>
</property>
<property name="text">
<string>Reset</string>
</property>
</widget>
</item>
</layout>
</widget>
</widget>
<widget class="QMenuBar" name="menuBar">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>544</width>
<height>25</height>
</rect>
</property>
</widget>
<widget class="QToolBar" name="mainToolBar">
<attribute name="toolBarArea">
<enum>TopToolBarArea</enum>
</attribute>
<attribute name="toolBarBreak">
<bool>false</bool>
</attribute>
</widget>
<widget class="QStatusBar" name="statusBar"/>
</widget>
<layoutdefault spacing="6" margin="11"/>
<resources/>
<connections/>
</ui>

And this is the urbi code.

Loads the main class roboArm with a variable arm which is used to call the roboArm.u class
start.u
load("roboArm.u");
var arm = roboArm.new;

This is the main class which controls the robotic arm. We first load the dynamixel.u library which we use for our servo control
roboArm.u
load("dynamixel.u");
var Global.d = Dynamixel.new;
var range = 0.1;

Creates a variable server which is used to connect to the C++ file to be able to receive data through an external application
var server = Server.new;
at (server.connection?(var socket))
at (socket.received?(var data)){
switch (data)
{
Each case commands command corresponds to a message sent from the external C++ application which in return executes a function from the roboArm.u file
case "reset":
echo ("Reset");
arm.reset;
case "demo":
echo ("Demo");
arm.sM;
case "record":
arm.stopRecording = false;
echo ("Recording");
arm.record;
case "stopRecording":
arm.stopRecording = true;
arm.record;
echo ("Record stopped");
case "play":
echo ("Playing");
arm.player;
case "play1":
echo ("Playing");
arm.player1;
case "play2":
echo ("Playing");
arm.player2;
case "play3":
echo ("Playing");
arm.player3;
case "s1_move_up":

The IF statements represent the boundaries of the roboric arm while manually moving it with a keyboard. It states that if any of the servos go out of the predefined position, then stop moving them.
if(arm.shoulder1.position+range <= arm.shoulder1.upperPos){
echo ("Move up");
arm.shoulder1.targetPos = arm.shoulder1.position+range;
}
case "s1_move_down":
if(arm.shoulder1.position-range >= arm.shoulder1.lowerPos){
echo ("Move Down");
arm.shoulder1.targetPos = arm.shoulder1.position-range;
}
case "s2_move_up":
if(arm.shoulder2.position+range <= arm.shoulder2.iniPos){
echo ("Move up");
arm.shoulder2.targetPos = arm.shoulder2.position+range;
}
case "s2_move_down":
if(arm.shoulder2.position-range >= arm.shoulder2.lowerPos){
echo ("Move Down");
arm.shoulder2.targetPos = arm.shoulder2.position-range;
}
case "s3_move_up":
if(arm.shoulderRot.position+range <= arm.shoulderRot.maxRight){
echo ("Move up");
arm.shoulderRot.targetPos = arm.shoulderRot.position+range;
}
case "s3_move_down":
if(arm.shoulderRot.position-range >= arm.shoulderRot.maxLeft){
echo ("Move Down");
arm.shoulderRot.targetPos = arm.shoulderRot.position-range;
}
case "e_move_up":
if(arm.elbow.position+range <= arm.elbow.iniPos){
echo ("Move up");
arm.elbow.targetPos = arm.elbow.position+range;
}
case "e_move_down":
if(arm.elbow.position-range >= arm.elbow.lowerPos){
echo ("Move Down");
arm.elbow.targetPos = arm.elbow.position-range;
}
};
};

server.listen("localhost", "5353");

class roboArm
{

The init() function represents where all variables and positions are defined. It also presents where each servo is called.
function init()
{
var speed = 2;


var this.stopRecording = false;

var this.shoulder1 = Dynamixel.Device.new(d,17,Dynamixel.DeviceDesc.AX12);
var this.shoulder2 = Dynamixel.Device.new(d,9,Dynamixel.DeviceDesc.AX12);
var this.shoulderRot = Dynamixel.Device.new(d,2,Dynamixel.DeviceDesc.AX12);
var this.elbow = Dynamixel.Device.new(d,6,Dynamixel.DeviceDesc.AX12);


this.shoulder1.targetSpeed = speed;
this.shoulder2.targetSpeed = speed;
this.shoulderRot.targetSpeed = speed;
this.elbow.targetSpeed = speed;


var this.shoulder1.iniPos = -1.47;
var this.shoulder1.upperPos = 1.6;
var this.shoulder1.lowerPos = -2;


var this.shoulder2.iniPos = 1.27;
var this.shoulder2.lowerPos = -1.21;


var this.shoulderRot.iniPos = -0.03;
var this.shoulderRot.maxLeft = -1.45;
var this.shoulderRot.maxRight = 0.57;


var this.elbow.iniPos = 1.58;
var this.elbow.lowerPos = -0.57;
};

The reset functions resets each servo to it's initial position when executed
function reset()
{
var delay = 1;
this.shoulder1.targetPos = this.shoulder1.iniPos;
sleep(delay);
this.shoulder2.targetPos = this.shoulder2.iniPos;
sleep(delay);
this.shoulderRot.targetPos = this.shoulderRot.iniPos;
sleep(delay);
this.elbow.targetPos = this.elbow.iniPos;
sleep(delay);
unlock();
};

The unlock functions unlocks each servo because once a speed is defined, or a servo has been moved, the servo gets locked and cannot be manually moved. We require manual movements of the servos whenever we want to record a script
function unlock(){
this.shoulderRot.load = 0;
this.elbow.load = 0;
this.shoulder2.load = 0;
this.shoulder1.load = 0;
};

the sM function represents the demo that we have played in class. This function represents all the movements of each servo with its minimum and maximum positions. It is used to test if each servo is functioning correctly
function sM()
{
var delay = 0.5;
var range = 0.05;
reset();
sleep(delay);
while(this.shoulder1.position<this.shoulder1.upperPos-range)
this.shoulder1.targetPos = this.shoulder1.upperPos;
sleep(delay);
while(this.shoulder1.position>this.shoulder1.lowerPos+range)
this.shoulder1.targetPos = this.shoulder1.lowerPos;
sleep(delay);
while(this.shoulder1.position<this.shoulder1.iniPos-range)
this.shoulder1.targetPos = this.shoulder1.iniPos;
sleep(delay);


while(this.shoulder2.position>this.shoulder2.lowerPos+range)
this.shoulder2.targetPos = this.shoulder2.lowerPos;
sleep(delay);
while(this.shoulder2.position<this.shoulder2.iniPos-range)
this.shoulder2.targetPos = this.shoulder2.iniPos;
sleep(delay);


while(this.shoulderRot.position>this.shoulderRot.maxLeft+range)
this.shoulderRot.targetPos = this.shoulderRot.maxLeft;
sleep(delay);
while(this.shoulderRot.position<this.shoulderRot.maxRight-range)
this.shoulderRot.targetPos = this.shoulderRot.maxRight;
sleep(delay);
while(this.shoulderRot.position>this.shoulderRot.iniPos+range)
this.shoulderRot.targetPos = this.shoulderRot.iniPos;
sleep(delay);


while(this.elbow.position>this.elbow.lowerPos+range)
this.elbow.targetPos = this.elbow.lowerPos;
sleep(delay);
while(this.elbow.position<this.elbow.iniPos-range)
this.elbow.targetPos = this.elbow.iniPos;
sleep(delay);


reset();
};

simpleMovement2 and 3 are the same as the sM function, but shorter. They only execute a particular servo
function simpleMovement2()
{
var delay = 0.1;
this.shoulder1.targetPos = this.shoulder1.upperPos;
sleep(delay);
this.shoulder2.targetPos = this.shoulder2.iniPos;
};


function simpleMovement3()
{
var delay = 0.1;
this.shoulder1.targetPos = this.shoulder1.upperPos;
sleep(delay);
this.shoulderRot.targetPos = this.shoulderRot.maxLeft;
sleep(delay);
this.elbow.targetPos = this.elbow.lowerPos;
};

The player function is used to play the recorded script. This is the main player function which is used to play the newest recorded script
function player(){
reset;
sleep(1s);

var is = InputStream.new(File.new("output.txt"));

var line;

reading:every(0.01) {
line = is.getLine.acceptVoid;
if (line.isVoid) {
reading.stop();
};
echo(line);
var ns = line.split(",");

Here we have set boundaries so that the function plays only the variables which are within our defined limits for the servos.
if(ns[0].asFloat()<=this.shoulder1.upperPos && ns[0].asFloat()>=this.shoulder1.lowerPos)
this.shoulder1.targetPos = ns[0].asFloat();


if(ns[1].asFloat()<=this.shoulder2.iniPos && ns[1].asFloat()>=this.shoulder2.lowerPos)
this.shoulder2.targetPos = ns[1].asFloat();


if(ns[2].asFloat()<=this.shoulderRot.maxRight && ns[2].asFloat()>=this.shoulderRot.maxLeft)
this.shoulderRot.targetPos = ns[2].asFloat();


if(ns[3].asFloat()<=this.elbow.iniPos && ns[3].asFloat()>=this.elbow.lowerPos)
this.elbow.targetPos = ns[3].asFloat();
};
};

player1, 2 and 3 are the same functions as the initial player, but here we have stored 3 recorded scripts which cannot be modified, unless the script has been overwritten
function player1(){
reset;
sleep(1s);

var is = InputStream.new(File.new("output1.txt"));

var line;

reading:every(0.01) {
line = is.getLine.acceptVoid;
if (line.isVoid) {
reading.stop();
};
echo(line);
var ns = line.split(",");
if(ns[0].asFloat()<=this.shoulder1.upperPos && ns[0].asFloat()>=this.shoulder1.lowerPos)
this.shoulder1.targetPos = ns[0].asFloat();


if(ns[1].asFloat()<=this.shoulder2.iniPos && ns[1].asFloat()>=this.shoulder2.lowerPos)
this.shoulder2.targetPos = ns[1].asFloat();


if(ns[2].asFloat()<=this.shoulderRot.maxRight && ns[2].asFloat()>=this.shoulderRot.maxLeft)
this.shoulderRot.targetPos = ns[2].asFloat();


if(ns[3].asFloat()<=this.elbow.iniPos && ns[3].asFloat()>=this.elbow.lowerPos)
this.elbow.targetPos = ns[3].asFloat();
};
};


function player2(){
reset;
sleep(1s);

var is = InputStream.new(File.new("output2.txt"));

var line;

reading:every(0.01) {
line = is.getLine.acceptVoid;
if (line.isVoid) {
reading.stop();
};
echo(line);
var ns = line.split(",");
if(ns[0].asFloat()<=this.shoulder1.upperPos && ns[0].asFloat()>=this.shoulder1.lowerPos)
this.shoulder1.targetPos = ns[0].asFloat();


if(ns[1].asFloat()<=this.shoulder2.iniPos && ns[1].asFloat()>=this.shoulder2.lowerPos)
this.shoulder2.targetPos = ns[1].asFloat();


if(ns[2].asFloat()<=this.shoulderRot.maxRight && ns[2].asFloat()>=this.shoulderRot.maxLeft)
this.shoulderRot.targetPos = ns[2].asFloat();


if(ns[3].asFloat()<=this.elbow.iniPos && ns[3].asFloat()>=this.elbow.lowerPos)
this.elbow.targetPos = ns[3].asFloat();
};
};


function player3(){
reset;
sleep(1s);

var is = InputStream.new(File.new("output3.txt"));

var line;

reading:every(0.01) {
line = is.getLine.acceptVoid;
if (line.isVoid) {
reading.stop();
};
echo(line);
var ns = line.split(",");
if(ns[0].asFloat()<=this.shoulder1.upperPos && ns[0].asFloat()>=this.shoulder1.lowerPos)
this.shoulder1.targetPos = ns[0].asFloat();


if(ns[1].asFloat()<=this.shoulder2.iniPos && ns[1].asFloat()>=this.shoulder2.lowerPos)
this.shoulder2.targetPos = ns[1].asFloat();


if(ns[2].asFloat()<=this.shoulderRot.maxRight && ns[2].asFloat()>=this.shoulderRot.maxLeft)
this.shoulderRot.targetPos = ns[2].asFloat();


if(ns[3].asFloat()<=this.elbow.iniPos && ns[3].asFloat()>=this.elbow.lowerPos)
this.elbow.targetPos = ns[3].asFloat();
};
};

This is the main record function. Whenever the function is executed and the servos manually moved by the user, it would record a new position every 0.01 seconds and store it only a .txt file which can later be played
function record(){
if(!stopRecording){
reset();
var o1 = OutputStream.new(File.create("output.txt"));
detach({
writing:every(0.01){
o1 << this.shoulder1.position << "," << this.shoulder2.position << "," << this.shoulderRot.position << "," << this.elbow.position << "\n";
};
});
}
else{
writing.stop;
}
};
};

The dynamixel.u is the library we have used. It has to be used as a default and it has been used for some variables such as declaring the servos and their ID's. We have added the code for the library because we have used it in our main urbiscript program and it is called at the very first line of our main script.
dynamixel.u
/*

• Copyright (C) 2010, Gostai S.A.S.

*
* This software is provided "as is" without warranty of any kind,

• either expressed or implied, including but not limited to the
• implied warranties of fitness for a particular purpose.

*
* See the LICENSE file for more information.
*/


if (!hasSlot("DynamixelPacket"))
loadModule("/home/thibbo/urbi-for-bioloid/bioloid.so");
/** Link to a bioloid CM-5 brick patched with gostai firmware.
*

• This code will also work with a direct connection to the dynamixel bus
• (using for instance the usb2dynamixel), if you disable all 'multiRead'
• optimization.

*
* usage:

• var Global.d = Dynamixel.new;
• var m = Dynamixel.Device.new(d, 3, Dynamixel.DeviceDesc.AX12);
• m.val = 100;
• var s = Dynamixel.Device.new(d, 100, Dynamixel.DeviceDesc.AXS1);
• s.buzzerIndex = 10;

*/

class Global.Dynamixel
{
function init(host="/dev/ttyUSB0", speed=57600, cm5 = false)
{
var this.io = IoService.new;
var this.socket = io.makeSocket;
var this.data = "";
var this.m = Mutex.new;
/ Are we connected throug a cm5 or directly on the bus.
var this.cm5 = cm5;
/ Mutex used to prevent multiple parallel operations.
var this.sendMutex = Mutex.new;
var this.reply = Event.new;
var this.lastDataSize = 1;
var this.parseTime = 1;
var this.writeTime = 1;
var this.debug = Tag.new;
/ For each write offset, pairs of (deviceId, value)
var this.writeCache = [[#]] * 256;
/ Time of last read for given offset.
var this.readTime = [time] * 256;
/ Cache of read values.
var this.readValues = 256.seq.map(function(x) {[0] * 256});
/ List of active device IDs
var this.addresses = [];

socket.connectSerial(host, speed, false);
at(socket.error?)
wall("socket error");
};

Write the list of (address, val) in addrVal to index 'idx'.
function multiWrite(addrVal, idx, size)
{
var addresses = addrVal.map(function(x) { x[0]})|
var vals = addrVal.map(function(x) { x[1]});
No reply to multiwrite packets
sendMutex:
socket.syncWrite(DynamixelPacket.multiWrite(addresses, idx,size, vals))
};

Get one packet
function syncGetPacket(dataSize=1)
{
var res|
var p|
var clen|
do(socket)
{
p = ""|
while| (p != "\xFF")
p = read(1)|
p += read(3)|
if (p[1] != "\xff")
{
wall("fail: " + p.asPrintable)|
return nil|
}|
clen = p[3].toAscii|
p += read(clen)|
res = DynamixelPacket.parse(p, dataSize)|
}|
res
};

Write to one motor.
function write(address, idx, size, val)
{
sendMutex: {
socket.syncWrite(DynamixelPacket.write(address, idx, size, val))|
if (idx != 254)
syncGetPacket
}
};

function read(address, idx, size)
{
sendMutex: {
if (address.isA(List))
socket.syncWrite(DynamixelPacket.multiRead(address, idx, size))
else
socket.syncWrite(DynamixelPacket.read(address, idx, size))|
var v = syncGetPacket(size)|
var res = {
if (v.isNil)
nil
else
if (v.size>2 && (address.isA(List) || v[0] == address))
{
if (!address.isA(List))
v[2]
else
{
v.removeFront|
v.removeFront|
v
}
}
else
nil
}|
res
}
};

/ Return the list of existing devices, with their type
function scan(maxid=253, verbose=true, fast=true)
{
var res = [];
debug.block;
var vals = [];
if (verbose)
wall("Scanning for devices, this can take up to 30 seconds.");
if (fast && cm5)
{
var alls = maxid.seq;
Big requests can take time
var chunk = 16;
for(var i: maxid/chunk) if (i*chunk <=maxid) {
vals += read(alls.range(i*chunk, (i*chunk+chunk).min(maxid)), 0, 2)|
}
}|
wall("done")|
for|(var i:maxid)
{
var v = {
if (!fast) read(i, 0, 2)
else
vals[i]
};
switch(v)
{
case 13:
res << (i, DeviceDesc.AXS1)|
if (verbose) wall("Found AXS1 at address " + i)
case 12:
res << (i, DeviceDesc.AX12)|
if (verbose) wall("Found AX12 at address " + i)
case 65535:
case (var x) if (x.isA(Float)):
res << (i, nil)
}
}|
wall("Scanning finished, " + res.size + " device(s) found.");
debug.unblock;
res
};

/** Create one device per scan result, using basename and the index.

• Also create an array with basename.
• Example: [(1, AX12)] will create motor1, motor[1].

*/
function instanciate(scanResult, where=Global)
{
// Extract model list.
var d = Dictionary.new;
for|(var s: scanResult)
d[s[1].basename] = 1;
for|(var k: d.keys)
where.setSlot(k+"s", [nil]*254);
for|(var s: scanResult)
{
var n = s[1].basename + s[0];
wall("Creating " + n);
var d = Device.new(this, s[0], s[1]);
where.setSlot(n, d);
where.getSlot(s[1].basename+"s")[s[0]] = d;
}
}
};

/** Device descriptions.
*

• Each device description is made of:
• -slots: list of (name, size, (readable, writable))
• Must be in the order of addresses, starting from 0, without gaps.
• -multiWrite: list of names for which to use optimized writing.
• -multiRead: list of names for which to use optimized reading.
• -pairs: dictionary targetSlot => (readslot, writeslot)
• create new slots merging tho two read/write ones
• - mapOut[]: Array of functions mapping from user values to dynamixel values
• - mapIn[addr]: Mapping from dynamixel values to user values.
• - range[addr]: (rangemin, rangemax) for value, or nil for none

*
* Only set multiWrite/Read for slots that will be read/written often.
*/
class Dynamixel.DeviceDesc {
function identity(x) { x};
function fallback { call.message};
var mapIn = [getSlot("identity")]*255;
var mapOut = [getSlot("identity")]*255;
var range = [nil] * 255;
function deviceSetup() {};
var R = (true, false);
var W = (false, true);
var RW = (true, true);
var NOOP = (false, false);
var base = [
key size read/write
(model, 2, R),
("version", 1, R),
(ID, 1, RW),
(bitRate, 1, RW),
(returnDelay,1, RW)];
class AX12: this
{
var slots = base + [
key size read/write
(cwLimit, 2, RW),
(ccwLimit, 2, RW),
(reserved, 1, NOOP),
(tempLimit, 1, RW),
(voltageMin, 1, RW),
(voltageMax, 1, RW),
(torqueMax, 2, RW),
(statusLevel,1, RW),
(alarmLED, 1, RW),
(alarmShutdown, 1, RW),
(reserved2, 1, NOOP),
(downCalib, 2, R),
(upCalib, 2, R),
("load", 1, RW),
(led, 1, RW),
(cwMargin, 1, RW),
(ccwMargin, 1, RW),
(cwSlope, 1, RW),
(ccwSlope, 1, RW),
(targetPos, 2, RW),
(targetSpeed,2, RW),
(torqueLimit,2, RW),
(position, 2, R),
(currentSpeed, 2, R),
(torque, 2, R),
(voltage, 1, R),
(temperature,1, R),
(registeredInstruction, 1, R),
(reserved3, 1, NOOP),
(moving, 1, R),
(lock, 1, RW),
(punch, 2, RW)
];
Trigger cow.
this.mapIn = mapIn.clone;
this.mapOut = mapOut.clone;
this.range = range.clone;
Motor position: 0-1023 -> -150-150deg
mapIn[6] = mapIn[8] = mapIn[36]
= function(x) { (x / 1023 - 0.5) * 300deg};
mapOut[6] = mapOut[8] = mapOut[30]
= function(x) { ((x / 300deg) + 0.5) * 1023};
range[6] = range[8] = range[36] = range[30] = (-150deg, 150deg);
Same thing for angle limit
Motor speed: 1023 = 114 RPM
0.0116697 = 1 / 1023 * 228 * pi / 60
mapIn[38] = function(x) { {if (x<1024) x else 1024 - x} * 0.0116697};
mapOut[32] = function(x) { var v = x/0.0116697| if (v<0) 1024-v else v };
range[38] = range[32] = (-228*pi/60, 228*pi/60);
var multiWrite = ["targetPos", "targetSpeed"];
var multiRead = ["position, speed, torque"];
var pairs = [
"val" => ("position", "targetPos"),
"speed" => ("currentSpeed", "targetSpeed")
];
var basename = "motor";
};
class AXS1: this {
var slots = base + [
(reserved5, 5, NOOP),
(tempLimit, 1, RW),
(voltageMin, 1, RW),
(voltageMax, 1, RW),
(reserved2, 2, NOOP),
(statusLevel,1, RW),
(reserved3, 3, NOOP),
(obstacleRange, 1, RW),
(lightRange, 1, RW),
(reserved4, 4, NOOP),
(IRLeft, 1, R),
(IRCenter, 1, R),
(IRRight, 1, R),
(lightLeft, 1, R),
(lightCenter,1, R),
(lightRight, 1, R),
(obstacle, 1, R),
(light, 1, R),
(reserved1, 1, R),
(soundVolume, 1, RW),
(soundVolumeMax, 1, RW),
(clapCount, 1, RW),
(clapTime, 2, RW),
(buzzerIndex,1, RW),
(buzzerTime, 1, RW),
(voltage, 1, R),
(temperature,1, R),
(registeredInstruction, 1, R),
(reserved1, 1, NOOP),
(IRData, 1, R),
(lock, 1, RW),
(IRRX0, 1, R),
(IRRX1, 1, R),
(IRTX0, 1, RW),
(IRTX1, 1, RW),
(obstacleCompare, 1, RW),
(lightCompare, 1, RW)
];
this.mapIn = mapIn.clone;
this.mapOut = mapOut.clone;
this.range = range.clone;
var multiWrite = [];
var multiRead = [];
var pairs = [];
var basename = "sensor";
}
};

class Dynamixel.Device {
function init(conn, address, model)
{
var this.conn = conn;
var this.address = address;
var this.deviceModel = model;
var this.handle = WeakPointer.new;
conn.addresses << address;
createSlots;
};
function createSlots()
{
var idx = 0;
for|(var item: deviceModel.slots)
{
(var name, var size, (var read, var write)) = item|
var myidx = idx |
var writeCache = conn.writeCache[myidx]|
idx += size|
if (!read && !write)
continue|
var v = UVar.new(this, name)|
v.owned = true|
if (read)
v.notifyAccess(handle, {
No multiread primivite without cm5.
if (item not in deviceModel.multiRead || conn.cm5)
closure()
{
var mi = deviceModel.mapIn[myidx] |
var raw = conn.read(address, myidx, size)|
if (!raw.isNil)
v.writeOwned(mi(raw))
}
else
closure()
{
var mi = deviceModel.mapIn[myidx] |
if (time - conn.readTime[myidx] < period)
v.writeOwned(mi(conn.readValues[myidx][address]))
else
{
conn.readTime[myidx] = time|
var vals = conn.read(conn.addresses, myidx, size)|
if (vals.isA(List)
&& vals.size == conn.addresses.size)
{
for|(var i: vals.size)
conn.readValues[myidx][conn.addresses[i]] =
vals[i]|
v.writeOwned(mi(conn.readValues[myidx][address]))
}
else
debug: wall("reply error: " +vals)
}
}}
)|
if (write)
v.notifyChangeOwned(handle, {
if (item not in deviceModel.multiWrite)
closure()
{
var mo = deviceModel.mapOut[myidx]|
conn.write(address, myidx, size, mo(v.val))
}
else
closure()
{
var mo = deviceModel.mapOut[myidx]|
var first = writeCache.empty |
writeCache << (address, mo(v.val))|
if (first)
detach({ sleep(period) |
conn.multiWrite(writeCache, myidx, size) |
writeCache.clear
})
}
})|
if (deviceModel.range[myidx])
{
setProperty(name, "rangemin", deviceModel.range[myidx][0])|
setProperty(name, "rangemax", deviceModel.range[myidx][1])
}
};
for|(var i: deviceModel.pairs)
{
var name = i[0];
var readName = i[1][0];
var writeName = i[1][1];
var v = UVar.new(this, name);
v.owned = true;
v.notifyChangeOwned(handle, closure() { updateSlot(writeName, v.val)});
v.notifyAccess(handle, closure() {v.writeOwned(getSlot(readName).accessor)});
if (! getProperty(writeName, "rangemin").isVoid)
{
setProperty(name, "rangemin", getProperty(writeName, "rangemin"))|
setProperty(name, "rangemax", getProperty(writeName, "rangemax"))
}
}
}
};

trace: trace.block;